#!/usr/bin/env python
PACKAGE = "actuatorcontroller_ros"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("K_R", double_t, 0, "Gain for Body Roll",   560,  0,   1000)
gen.add("Kd_R", double_t, 0, "Gain for Body Roll Velocity",   20,  0,   1000)
gen.add("K_P", double_t, 0, "Gain for Body Pitch",   670,  0,   1000)
gen.add("Kd_P", double_t, 0, "Gain for Body Pitch Velocity",   140,  0,   1000)
gen.add("K_Yaw", double_t, 0, "Gain for Body Yaw",   200,  0,   1000)
gen.add("Kd_Yaw", double_t, 0, "Gain for Body Yaw Velocity",   5,  0,   1000)

gen.add("K_X", double_t, 0, "Gain for Leg X Position",   280,  0,   1000)
gen.add("Kd_X", double_t, 0, "Gain for Leg X Velocity",   60,  0,   1000)
gen.add("K_Y", double_t, 0, "Gain for Leg Y Position",   10,  0,   1000)
gen.add("Kd_Y", double_t, 0, "Gain for Leg Y Velocity",   20,  0,   1000)
gen.add("K_Z", double_t, 0, "Gain for Body Height",   1380,  0,   2000)
gen.add("Kd_Z", double_t, 0, "Gain for Body Height Velocity",  160,  0,   1000)


gen.add("K_X_Body", double_t, 0, "Gain for Leg X Position",   0.0,  0,   1000)
gen.add("Kd_X_Body", double_t, 0, "Gain for Leg X Velocity",   310,  0,   1000)
gen.add("K_Y_Body", double_t, 0, "Gain for Leg Y Position",   520,  0,   1000)
gen.add("Kd_Y_Body", double_t, 0, "Gain for Leg Y Velocity",   320,  0,   1000)
gen.add("K_Z_Body", double_t, 0, "Gain for Body Height",   1380,  0,   2000)
gen.add("Kd_Z_Body", double_t, 0, "Gain for Body Height Velocity",  50,  0,   1000)

gen.add("K_R_Body", double_t, 0, "Gain for Leg X Position",   680,  0,   1000)
gen.add("Kd_R_Body", double_t, 0, "Gain for Leg X Velocity",   70,  0,   1000)
gen.add("K_P_Body", double_t, 0, "Gain for Leg Y Position",   640,  0,   1000)
gen.add("Kd_P_Body", double_t, 0, "Gain for Leg Y Velocity",   80,  0,   1000)
gen.add("K_Yaw_Body", double_t, 0, "Gain for Body Height",   0.0,  0,   2000)
gen.add("Kd_Yaw_Body", double_t, 0, "Gain for Body Height Velocity",  50,  0,   1000)


gen.add("ref_height" , double_t , 0 , "Adjust the Body Height" , -0.5 , -0.55 , -0.2)
gen.add("feedback_step_gains" , double_t , 0 , "Adjust How much the foot places forward I guess lol" , 2.01 , 0 , 3.0)
gen.add("desiredXVelocity" , double_t , 0 , "Desire forward velocity " , 0.0 , -0.5 , 0.5)
gen.add("desiredYVelocity" , double_t , 0 , "Desire sideway velocity" , 0.0 , -0.5 , 0.5)
gen.add("desiredYawVelocity" , double_t , 0 , "Deisre Yaw velocity" , 0.0 , -0.5 , 0.5)

gen.add("body_height_filter_ratio" , double_t , 0 , "I Don't even know, is this even needed" , 0.8 , 0.0 , 1.0)

gen.add("K_Swing_X" , double_t , 0 , "Swing Leg X Error Gain" , 200,  0,   1000)
gen.add("Kd_Swing_X" , double_t , 0 , "Swing Leg X Velocity Gain" , 25,  0,   1000)
gen.add("K_Swing_Y" , double_t , 0 , "Swing Leg Y Error Gain" , 200,  0,   1000)
gen.add("Kd_Swing_Y" , double_t , 0 , "Swing Leg Y Velocity Gain" , 25,  0,   1000)
gen.add("K_Swing_Z" , double_t , 0 , "Swing Leg Z Error Gain" , 200,  0,   2000)
gen.add("Kd_Swing_Z" , double_t , 0 , "Swing Leg Z Velocity Gain" , 25,  0,   1000)

gen.add("DM_R", double_t, 0, "Roll of Death Machine",   0,  -0.5,   0.5)
gen.add("DM_P", double_t, 0, "Pitch of Death Machine",   0,  -0.5,   0.5)



gen.add("S_X" , double_t , 2.4 , "OPT X  Gain" , 1,  0,   20)
gen.add("S_Y" , double_t , 3.0 , "OPT Y  Gain" , 1,  0,   20)
gen.add("S_Z" , double_t , 0.8 , "OPT Z  Gain" , 0.5,  0,   20)
gen.add("S_R" , double_t , 20 , "OPT Roll  Gain" , 1,  0,   20)
gen.add("S_P" , double_t , 20 , "OPT Pitch  Gain" , 1,  0,   20)
gen.add("S_Yaw" , double_t , 1.6 , "OPT Yaw  Gain" , 0.5,  0,   20)


size_enum = gen.enum([ gen.const("Stand ",      int_t, 0, "Standing Algorithem "),
                       gen.const("TrotWalk",     int_t, 1, "A 0.5 - 0.5 troting "),
                       gen.const("FreeWalk",     int_t, 2, "A 1/6 walking")] ,
                     "An enum to set size")

gen.add("Gaits", int_t, 0, "Pick your fight", 0, 0, 2, edit_method=size_enum)

exit(gen.generate(PACKAGE, "wow_doge", "pidtuning"))
